#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S4,     compass,             sensorI2CHiTechnicCompass)
#pragma config(Motor,  motorA,          lmotorA,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  motorB,          lmotorB,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  motorC,          loader,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_1,     dmotorA,       tmotorNormal, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     dmotorB,       tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     combine,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_2,     basketmo,      tmotorNormal, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    aim,                  tServoNormal)
#pragma config(Servo,  srvo_S1_C3_2,    ramp,                 tServoNormal)
#pragma config(Servo,  srvo_S1_C3_3,    flpusher,             tServoNormal)
#pragma config(Servo,  srvo_S1_C3_4,    rlpusher,             tServoNormal)
#pragma config(Servo,  srvo_S1_C3_5,    basket,               tServoNormal)
#pragma config(Servo,  srvo_S1_C3_6,    ,                     tServoNormal)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//JoystickDreiver.c is the magic behind the getJoytickSettings(), and a few other functions;
#include "JoystickDriver.c"
#include "HTMC-driver.h"

/*if(spunup == false)
	{
	  if(LLAUNCHER_KEY)
	    buttonWasPressed = true;
	  else if(!LLAUNCHER_KEY && buttonWasPressed)
	  {
	    loaderMotor(100);
	    timer = nPgmTime;
	    buttonWasPressed = false;
	    spunup = true;
	  }
  }
  else if(LLAUNCHER_KEY)
	  buttonWasPressed = true;
	else if(!LLAUNCHER_KEY && buttonWasPressed)
	  {
	    launchMotors(0,0);
	    loaderMotor(0);
	    buttonWasPressed = false;
	    spunup = false;
	  }*/

//Keymap
	//Joystick 1
	#define RAMP_KEY joy1Btn(5)
	#define COMBINE_KEY joy1Btn(6)

	//Joystick 2
	#define BASKETWEAKSERVO_UP_KEY joy2Btn(6)
	#define BASKETWEAKSERVO_DOWN_KEY joy2Btn(8)
	#define BASKETSERVO_UP_KEY joy2Btn(3)
	#define BASKETSERVO_DOWN_KEY joy2Btn(1)
	#define LOADER_REVERSE_KEY joy2Btn(4)
	#define FLPUSHER_KEY joy2Btn(5)
	#define LOADER_KEY joy2Btn(2)
	#define RLPUSHER_KEY joy2Btn(7)



//Other misc. definitions
#define TELEOP_TIME 900
#define CONTROLLER_DEAD_ZONE 20

//Declare Functions For Project
//Init is where we'll put anything that needs to be done before the round starts. EX Put servos in a certain position,
//etc.
void init();
void auto();

// *Motors() is shorthand for motor[lmotorA] = ... etc
void launchMotors(short m);
void driveMotors(short m1, short m2);
void combineMotor(short m);
void loaderMotor(short m);
void basketMotor(short m);

void init()
{
  servo[ramp] = 160;
  servo[aim] = 38;
  servo[flpusher] = 90;
  servo[rlpusher] = 166;
  servo[basket] = 140;
}

task main()
{
  init();
  waitForStart();
  auto();
}

void auto()
{
   int north = SensorValue[compass];
   int anglebuffer =0;
   int target = 0;
   int heading = 0;
   //Go forward @ 75 for .9s
   driveMotors(75,75);
   wait10Msec(70);
   //Brake for 20
   driveMotors(-100,-100);
   wait10Msec(20);
   //Start right-hand turn, wait 1.75s
   driveMotors(50,-50);
   heading = SensorValue[compass];
   target = heading + 75;
   if(target > 360)
     target = target - 360;
   while(heading != target)
   {
     heading = SensorValue[compass];
   }
   //Go straight @ 75 for .7s
   driveMotors(75,75);
   wait10Msec(70);
   //Light brake for .1s
   driveMotors(-20,-20);
   wait10Msec(10);
   driveMotors(1,1);
   //Launch balls
   launchMotors(100);
   wait10Msec(150);
   loaderMotor(75);
   wait10Msec(1000);
   while(true){}
}

void launchMotors(short m)
{
  motor[lmotorA]= motor[lmotorB] = m;
}

//Note that driveMotors will ignore values < CONTROLLER_DEAD_ZONE
void driveMotors(short m1, short m2)
{
  //Here we scale the joystick control values from (-127,127) to (-100,100) which is what the controller accepts.
  //There is also a deadzone of 10 either way to keep it stable. Due to an odd hardware glitch, we also have to
  //keep the motors at 1 power in order to keep them from misfiring
  if(abs(m1) >= CONTROLLER_DEAD_ZONE)
    motor[dmotorA] = m1/1.27;
  else
    motor[dmotorA] = 1;
  if(abs(m2) >= CONTROLLER_DEAD_ZONE)
    motor[dmotorB] = m2/1.27;
  else
    motor[dmotorB] = 1;
}

void combineMotor(short m)
{
  motor[combine] = m;
}

void loaderMotor(short m)
{
  motor[loader] = m;
}

void basketMotor(short m)
{
  motor[basketmo] = m;
}
